#include "bsp.h"


// 全局变量定义
BNO055_Data_t BNO055_Data;
BNO055_Data_t sensor_data;

/**
 * @brief 优化BNO055配置
 */
uint8_t BNO055_OptimizedInit(I2C_HandleTypeDef *hi2c)
{
    uint8_t config_mode = 0x00;
    uint8_t imu_mode = 0x08;
    uint8_t page_zero = 0x00;
    // 确保在PAGE 0
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x07,
                      I2C_MEMADD_SIZE_8BIT, &page_zero, 1, 10);
    // 切换到配置模式
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x3D,
                      I2C_MEMADD_SIZE_8BIT, &config_mode, 1, 10);
    HAL_Delay(25);
    // 设置陀螺仪带宽（减少噪声）
    uint8_t gyro_bw = 0x0B;  // 23Hz带宽，降低噪声
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x10,
                      I2C_MEMADD_SIZE_8BIT, &gyro_bw, 1, 10);
    // 设置陀螺仪操作模式（正常模式）
    uint8_t gyro_opmode = 0x00;  // 正常模式
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x11,
                      I2C_MEMADD_SIZE_8BIT, &gyro_opmode, 1, 10);
    // 切换到IMU模式
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x3D,
                      I2C_MEMADD_SIZE_8BIT, &imu_mode, 1, 10);
    HAL_Delay(30);
    return 0;
}

/**
 * @brief 初始化BNO055传感器
 * @param hi2c I2C句柄指针
 * @return 0:成功, 1:通信失败, 2:芯片ID不匹配
 */
uint8_t BNO055_Init(I2C_HandleTypeDef *hi2c)
{
    uint8_t chip_id = 0;
    uint8_t config_mode = CONFIG_MODE;
    uint8_t normal_power = POWER_MODE_NORMAL;
    uint8_t imu_mode = IMU_MODE;
    uint8_t ndof_mode = 0x0C;

    // 1. 读取芯片ID验证通信
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_CHIP_ID,
                         I2C_MEMADD_SIZE_8BIT, &chip_id, 1, HAL_MAX_DELAY) != HAL_OK) {
        debug_print("BNO055 communication failed\r\n");
        return 1; // 通信失败
    }

    // 2. 验证芯片ID
    if (chip_id != BNO055_CHIP_ID) {
        debug_print("BNO055 chip ID mismatch\r\n");
        return 2; // 芯片ID不匹配
    }

    // 3. 切换到配置模式
    if (HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, BNO055_REG_OPR_MODE,
                          I2C_MEMADD_SIZE_8BIT, &config_mode, 1, HAL_MAX_DELAY) != HAL_OK) {
        debug_print("BNO055 communication failed\r\n");
        return 1;
    }

    HAL_Delay(20);

    // 4. 设置电源模式为正常模式
    if (HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, BNO055_REG_PWR_MODE,
                          I2C_MEMADD_SIZE_8BIT, &normal_power, 1, HAL_MAX_DELAY) != HAL_OK) {
        debug_print("BNO055 communication failed\r\n");
        return 1;
    }

    HAL_Delay(10);

    // 5. 设置操作模式为NDOF
    if (HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, BNO055_REG_OPR_MODE,
                          I2C_MEMADD_SIZE_8BIT, &ndof_mode, 1, HAL_MAX_DELAY) != HAL_OK) {
        debug_print("BNO055 communication failed\r\n");
        return 1;
    }

    HAL_Delay(30);
    // 6. 初始化数据结构
    memset(&BNO055_Data, 0, sizeof(BNO055_Data_t));
    debug_print("BNO055 initialization successful\r\n");
    return 0; // 成功
}

void BNO055_loop(void)
{
    // 读取所有传感器数据
    if (BNO055_ReadAccel(&hi2c1, &sensor_data) == HAL_OK &&
            BNO055_ReadEuler(&hi2c1, &sensor_data) == HAL_OK &&
            BNO055_ReadQuaternion(&hi2c1, &sensor_data) == HAL_OK) {
        // 使用传感器数据
        // sensor_data.AccX, sensor_data.GyroX, etc.
    }
}

void BNO055_ReadGyro(I2C_HandleTypeDef *hi2c, float gyro[3])
{
    uint8_t buffer[6];
    HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x14,
                     I2C_MEMADD_SIZE_8BIT, buffer, 6, HAL_MAX_DELAY);
    // 转换为实际值 (单位: rad/s)
    // BNO055角速度单位: 1 dps = 16 LSB, 需要转换为rad/s
    BNO055_Data.gyro1 = ((int16_t)((buffer[1] << 8) | buffer[0])) / 16.0 * (M_PI / 180.0); // X
    BNO055_Data.gyro2 = ((int16_t)((buffer[3] << 8) | buffer[2])) / 16.0 * (M_PI / 180.0); // Y
    BNO055_Data.gyro3 = ((int16_t)((buffer[5] << 8) | buffer[4])) / 16.0 * (M_PI / 180.0); // Z
}

/**
 * @brief 读取加速度计数据
 * @param hi2c I2C句柄指针
 * @param data 数据结构体指针
 * @return HAL状态
 */
uint8_t BNO055_ReadAccel(I2C_HandleTypeDef *hi2c, BNO055_Data_t *data)
{
    uint8_t buffer[6];
    HAL_StatusTypeDef status;
    status = HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_ACCEL_DATA,
                              I2C_MEMADD_SIZE_8BIT, buffer, 6, HAL_MAX_DELAY);

    if (status == HAL_OK && data != NULL) {
        data->AccX = (int16_t)((buffer[1] << 8) | buffer[0]);
        data->AccY = (int16_t)((buffer[3] << 8) | buffer[2]);
        data->AccZ = (int16_t)((buffer[5] << 8) | buffer[4]);
        // 同时更新全局数据结构
        BNO055_Data.AccX = data->AccX;
        BNO055_Data.AccY = data->AccY;
        BNO055_Data.AccZ = data->AccZ;
    }

    //    debug_print("Euler (deg): AccX=%.2f, AccY=%.2f, AccZ=%.2f\r\n",
    //                data->AccX / 16.0, data->AccY / 16.0, data->AccZ / 16.0);
    return status;
}

/**
 * @brief 读取欧拉角数据
 * @param hi2c I2C句柄指针
 * @param data 数据结构体指针
 * @return HAL状态
 */
uint8_t BNO055_ReadEuler(I2C_HandleTypeDef *hi2c, BNO055_Data_t *data)
{
    uint8_t buffer[6];
    HAL_StatusTypeDef status;
    status = HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_EULER_DATA,
                              I2C_MEMADD_SIZE_8BIT, buffer, 6, HAL_MAX_DELAY);

    if (status == HAL_OK && data != NULL) {
        data->GyroX = (int16_t)((buffer[1] << 8) | buffer[0]);  // Heading
        data->GyroY = (int16_t)((buffer[3] << 8) | buffer[2]);  // Roll
        data->GyroZ = (int16_t)((buffer[5] << 8) | buffer[4]);  // Pitch
        // 同时更新全局数据结构
        BNO055_Data.GyroX = data->GyroX;
        BNO055_Data.GyroY = data->GyroY;
        BNO055_Data.GyroZ = data->GyroZ;
    }

    //    debug_print("Euler (deg): GyroX=%.2f, GyroY=%.2f, GyroZ=%.2f\r\n",
    //                data->GyroX / 16.0, data->GyroY / 16.0, data->GyroZ / 16.0);
    return status;
}

/**
 * @brief 读取四元数数据
 * @param hi2c I2C句柄指针
 * @param data 数据结构体指针
 * @return HAL状态
 */
uint8_t BNO055_ReadQuaternion(I2C_HandleTypeDef *hi2c, BNO055_Data_t *data)
{
    uint8_t buffer[8];
    HAL_StatusTypeDef status;
    status = HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_QUAT_DATA,
                              I2C_MEMADD_SIZE_8BIT, buffer, 8, HAL_MAX_DELAY);

    if (status == HAL_OK && data != NULL) {
        data->quat1 = (int16_t)((buffer[1] << 8) | buffer[0]);  // W
        data->quat2 = (int16_t)((buffer[3] << 8) | buffer[2]);  // X
        data->quat3 = (int16_t)((buffer[5] << 8) | buffer[4]);  // Y
        data->quat4 = (int16_t)((buffer[7] << 8) | buffer[6]);  // Z
        // 同时更新全局数据结构
        BNO055_Data.quat1 = data->quat1;
        BNO055_Data.quat2 = data->quat2;
        BNO055_Data.quat3 = data->quat3;
        BNO055_Data.quat4 = data->quat4;
    }

    //    debug_print("Euler (deg): quat1=%.2f, quat2=%.2f, quat3=%.2f, quat4=%.2f\r\n",
    //                data->quat1 / 16.0, data->quat2 / 16.0, data->quat3 / 16.0, data->quat4 / 16.0);
    return status;
}

/**
 * @brief 读取用于FAST-LIO的IMU数据（角速度+线性加速度）
 * @param hi2c I2C句柄指针
 * @param gyro 角速度数组 [x, y, z]，单位: rad/s
 * @param accel 加速度数组 [x, y, z]，单位: m/s²
 * @return HAL状态
 */
uint8_t BNO055_ReadIMUForFASTLIO(I2C_HandleTypeDef *hi2c, float gyro[3], float accel[3])
{
    uint8_t gyro_buffer[6], accel_buffer[6];

    // 读取角速度数据
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x14,
                         I2C_MEMADD_SIZE_8BIT, gyro_buffer, 6, HAL_MAX_DELAY) != HAL_OK) {
        return 1;
    }

    // 读取加速度数据
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x08,
                         I2C_MEMADD_SIZE_8BIT, accel_buffer, 6, HAL_MAX_DELAY) != HAL_OK) {
        return 1;
    }

    // 转换角速度数据 (rad/s)
    gyro[0] = ((int16_t)((gyro_buffer[1] << 8) | gyro_buffer[0])) / 16.0 * (M_PI / 180.0);
    gyro[1] = ((int16_t)((gyro_buffer[3] << 8) | gyro_buffer[2])) / 16.0 * (M_PI / 180.0);
    gyro[2] = ((int16_t)((gyro_buffer[5] << 8) | gyro_buffer[4])) / 16.0 * (M_PI / 180.0);
    // 转换加速度数据 (m/s²)
    accel[0] = ((int16_t)((accel_buffer[1] << 8) | accel_buffer[0])) / 100.0;
    accel[1] = ((int16_t)((accel_buffer[3] << 8) | accel_buffer[2])) / 100.0;
    accel[2] = ((int16_t)((accel_buffer[5] << 8) | accel_buffer[4])) / 100.0;
    // 更新全局数据结构
    BNO055_Data.gyro1 = gyro[0];
    BNO055_Data.gyro2 = gyro[1];
    BNO055_Data.gyro3 = gyro[2];
    BNO055_Data.AccX = accel[0] * 100.0;  // 转换为原始值存储
    BNO055_Data.AccY = accel[1] * 100.0;
    BNO055_Data.AccZ = accel[2] * 100.0;
    return 0;
}

/**
 * @brief 读取校准状态
 * @param hi2c I2C句柄指针
 * @param calib 校准状态结构体指针
 * @return HAL状态
 */
uint8_t BNO055_ReadCalibStatus(I2C_HandleTypeDef *hi2c, BNO055_CalibStatus_t *calib)
{
    uint8_t calib_status;
    HAL_StatusTypeDef ret;
    ret = HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_CALIB_STAT,
                           I2C_MEMADD_SIZE_8BIT, &calib_status, 1, HAL_MAX_DELAY);

    if (ret == HAL_OK && calib != NULL) {
        calib->system = (calib_status >> 6) & 0x03;
        calib->gyro   = (calib_status >> 4) & 0x03;
        calib->accel  = (calib_status >> 2) & 0x03;
        calib->mag    = calib_status & 0x03;
    }

    return ret;
}

void CheckCalibrationStatus(void)
{
    BNO055_CalibStatus_t calib;

    if (BNO055_ReadCalibStatus(&hi2c1, &calib) == HAL_OK) {
        debug_print("Calibration status - System:%d, Gyroscope:%d, Accelerometer:%d, Magnetometer:%d\r\n",
                    calib.system, calib.gyro, calib.accel, calib.mag);

        // All calibration levels should reach 3 for optimal performance
        if (calib.system < 3 || calib.mag < 3) {
            debug_print("Warning: Sensor needs calibration!\r\n");
        }
    }
}

/**
 * @brief 诊断函数
 * @param hi2c I2C句柄指针
 */
void BNO055_Diagnostic(I2C_HandleTypeDef *hi2c)
{
    uint8_t chip_id, op_mode;
    BNO055_CalibStatus_t calib;

    // 1. 检查芯片ID
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_CHIP_ID,
                         I2C_MEMADD_SIZE_8BIT, &chip_id, 1, HAL_MAX_DELAY) == HAL_OK) {
        debug_print("Chip ID: 0x%02X %s\r\n", chip_id,
                    (chip_id == BNO055_CHIP_ID) ? "(normal)" : "(abnormal!)");
    }

    // 2. 检查操作模式
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_OPR_MODE,
                         I2C_MEMADD_SIZE_8BIT, &op_mode, 1, HAL_MAX_DELAY) == HAL_OK) {
        debug_print("Operation mode: 0x%02X\r\n", op_mode);
    }

    // 3. 检查校准状态
    if (BNO055_ReadCalibStatus(hi2c, &calib) == HAL_OK) {
        debug_print("Calibration - System:%d, Gyro:%d, Accel:%d, Mag:%d\r\n",
                    calib.system, calib.gyro, calib.accel, calib.mag);
    }

    // 4. 读取并显示欧拉角数据
    BNO055_DebugReadAccel(hi2c);
    HAL_Delay(10);
    BNO055_DebugReadGyro(hi2c);  // 新增陀螺仪数据
    HAL_Delay(10);
    //    BNO055_DebugReadEuler(hi2c);
    //	HAL_Delay(10);
    //	BNO055_DebugReadQuaternion(hi2c);
    //	HAL_Delay(10);
    BNO055_DebugReadLinearAccel(hi2c);
    HAL_Delay(10);
    BNO055_DebugReadGravity(hi2c);
    HAL_Delay(10);
}

/**
 * @brief 调试函数：打印陀螺仪原始数据, 角速度
 * @param hi2c I2C句柄指针
 */
void BNO055_DebugReadGyro(I2C_HandleTypeDef *hi2c)
{
    uint8_t buffer[6];

    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x14,  // 陀螺仪数据寄存器地址
                         I2C_MEMADD_SIZE_8BIT, buffer, 6, HAL_MAX_DELAY) == HAL_OK) {
        debug_print("Gyro raw data: ");

        for (int i = 0; i < 6; i++) {
            debug_print("%02X ", buffer[i]);
        }

        debug_print("\r\n");
        // 转换为原始值
        int16_t gyro_x = (int16_t)((buffer[1] << 8) | buffer[0]);
        int16_t gyro_y = (int16_t)((buffer[3] << 8) | buffer[2]);
        int16_t gyro_z = (int16_t)((buffer[5] << 8) | buffer[4]);
        debug_print("Gyro (raw): X=%d, Y=%d, Z=%d\r\n", gyro_x, gyro_y, gyro_z);
        // 转换为度/秒 (dps)
        debug_print("Gyro (dps): X=%.2f, Y=%.2f, Z=%.2f\r\n",
                    gyro_x / 16.0, gyro_y / 16.0, gyro_z / 16.0);
        // 转换为弧度/秒 (rad/s) - 用于FAST-LIO
        debug_print("Gyro (rad/s): X=%.4f, Y=%.4f, Z=%.4f\r\n",
                    gyro_x / 16.0 * (M_PI / 180.0),
                    gyro_y / 16.0 * (M_PI / 180.0),
                    gyro_z / 16.0 * (M_PI / 180.0));
    } else {
        debug_print("Failed to read gyroscope data!\r\n");
    }
}

/**
 * @brief 调试函数：打印加速度原始数据
 * @param hi2c I2C句柄指针
 */
void BNO055_DebugReadAccel(I2C_HandleTypeDef *hi2c)
{
    uint8_t buffer[6];

    // 修正：正确检查返回值
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x08,
                         I2C_MEMADD_SIZE_8BIT, buffer, 6, HAL_MAX_DELAY) == HAL_OK) {
        debug_print("Accel raw data: ");

        for (int i = 0; i < 6; i++) {
            debug_print("%02X ", buffer[i]);
        }

        debug_print("\r\n");
        int16_t AccX = (int16_t)((buffer[1] << 8) | buffer[0]);
        int16_t AccY = (int16_t)((buffer[3] << 8) | buffer[2]);
        int16_t AccZ = (int16_t)((buffer[5] << 8) | buffer[4]);
        debug_print("Accel (raw): X=%d, Y=%d, Z=%d\r\n", AccX, AccY, AccZ);
        debug_print("Accel (m/s2): X=%.2f, Y=%.2f, Z=%.2f\r\n",
                    AccX / 100.0, AccY / 100.0, AccZ / 100.0); // 注意单位转换
    } else {
        debug_print("Failed to read acceleration data!\r\n");
    }
}

void BNO055_DebugReadEuler(I2C_HandleTypeDef *hi2c)
{
    uint8_t buffer[6];

    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_EULER_DATA,
                         I2C_MEMADD_SIZE_8BIT, buffer, 6, HAL_MAX_DELAY) == HAL_OK) {
        debug_print("Euler raw data: ");

        for (int i = 0; i < 6; i++) {
            debug_print("%02X ", buffer[i]);
        }

        debug_print("\r\n");
        int16_t heading = (int16_t)((buffer[1] << 8) | buffer[0]);
        int16_t roll = (int16_t)((buffer[3] << 8) | buffer[2]);
        int16_t pitch = (int16_t)((buffer[5] << 8) | buffer[4]);
        debug_print("Euler (raw): heading=%d, roll=%d, pitch=%d\r\n", heading, roll, pitch);
        debug_print("Euler (deg): heading=%.2f, roll=%.2f, pitch=%.2f\r\n",
                    heading / 16.0, roll / 16.0, pitch / 16.0);
    }
}

/**
 * @brief 调试函数：打印四元数原始数据
 * @param hi2c I2C句柄指针
 */
void BNO055_DebugReadQuaternion(I2C_HandleTypeDef *hi2c)
{
    uint8_t buffer[8];

    // 修正：正确检查返回值
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_QUAT_DATA,
                         I2C_MEMADD_SIZE_8BIT, buffer, 8, HAL_MAX_DELAY) == HAL_OK) {
        debug_print("Quaternion raw data: ");

        for (int i = 0; i < 8; i++) {
            debug_print("%02X ", buffer[i]);
        }

        debug_print("\r\n");
        int16_t quat1 = (int16_t)((buffer[1] << 8) | buffer[0]);  // W
        int16_t quat2 = (int16_t)((buffer[3] << 8) | buffer[2]);  // X
        int16_t quat3 = (int16_t)((buffer[5] << 8) | buffer[4]);  // Y
        int16_t quat4 = (int16_t)((buffer[7] << 8) | buffer[6]);  // Z
        debug_print("Quat (raw): W=%d, X=%d, Y=%d, Z=%d\r\n", quat1, quat2, quat3, quat4);
        debug_print("Quat (unit): W=%.4f, X=%.4f, Y=%.4f, Z=%.4f\r\n",
                    quat1 / 16384.0, quat2 / 16384.0,
                    quat3 / 16384.0, quat4 / 16384.0); // 正确单位转换
    } else {
        debug_print("Failed to read quaternion data!\r\n");
    }
}

/**
 * @brief 读取线性加速度数据（去除重力影响）
 * @param hi2c I2C句柄指针
 */
void BNO055_DebugReadLinearAccel(I2C_HandleTypeDef *hi2c)
{
    uint8_t buffer[6];

    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x28,  // 线性加速度寄存器
                         I2C_MEMADD_SIZE_8BIT, buffer, 6, HAL_MAX_DELAY) == HAL_OK) {
        debug_print("Linear Accel raw data: ");

        for (int i = 0; i < 6; i++) {
            debug_print("%02X ", buffer[i]);
        }

        debug_print("\r\n");
        int16_t lin_acc_x = (int16_t)((buffer[1] << 8) | buffer[0]);
        int16_t lin_acc_y = (int16_t)((buffer[3] << 8) | buffer[2]);
        int16_t lin_acc_z = (int16_t)((buffer[5] << 8) | buffer[4]);
        debug_print("Linear Accel (raw): X=%d, Y=%d, Z=%d\r\n", lin_acc_x, lin_acc_y, lin_acc_z);
        debug_print("Linear Accel (m/s2): X=%.2f, Y=%.2f, Z=%.2f\r\n",
                    lin_acc_x / 100.0, lin_acc_y / 100.0, lin_acc_z / 100.0);
    } else {
        debug_print("Failed to read linear acceleration data!\r\n");
    }
}

/**
 * @brief 读取重力向量数据
 * @param hi2c I2C句柄指针
 */
void BNO055_DebugReadGravity(I2C_HandleTypeDef *hi2c)
{
    uint8_t buffer[6];

    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x2E,  // 重力向量寄存器
                         I2C_MEMADD_SIZE_8BIT, buffer, 6, HAL_MAX_DELAY) == HAL_OK) {
        debug_print("Gravity raw data: ");

        for (int i = 0; i < 6; i++) {
            debug_print("%02X ", buffer[i]);
        }

        debug_print("\r\n");
        int16_t grav_x = (int16_t)((buffer[1] << 8) | buffer[0]);
        int16_t grav_y = (int16_t)((buffer[3] << 8) | buffer[2]);
        int16_t grav_z = (int16_t)((buffer[5] << 8) | buffer[4]);
        debug_print("Gravity (raw): X=%d, Y=%d, Z=%d\r\n", grav_x, grav_y, grav_z);
        debug_print("Gravity (m/s2): X=%.2f, Y=%.2f, Z=%.2f\r\n",
                    grav_x / 100.0, grav_y / 100.0, grav_z / 100.0);
    } else {
        debug_print("Failed to read gravity data!\r\n");
    }
}

/**
 * @brief 加速度计校准程序
 * @param hi2c I2C句柄指针
 */
void BNO055_CalibrateAccelerometer(I2C_HandleTypeDef *hi2c)
{
    BNO055_CalibStatus_t calib;
    uint8_t original_mode;
    debug_print("Starting accelerometer calibration...\r\n");
    debug_print("Place sensor in 6 different stable positions for 10 seconds each:\r\n");
    debug_print("1. Z up, 2. Z down, 3. X up, 4. X down, 5. Y up, 6. Y down\r\n");
    // 保存当前模式
    HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, BNO055_REG_OPR_MODE,
                     I2C_MEMADD_SIZE_8BIT, &original_mode, 1, HAL_MAX_DELAY);
    // 切换到配置模式
    uint8_t config_mode = CONFIG_MODE;
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, BNO055_REG_OPR_MODE,
                      I2C_MEMADD_SIZE_8BIT, &config_mode, 1, HAL_MAX_DELAY);
    HAL_Delay(20);
    // 开始校准 - 清除校准数据
    uint8_t clear_calib = 0x00;
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x55,
                      I2C_MEMADD_SIZE_8BIT, &clear_calib, 1, HAL_MAX_DELAY);
    // 切换回NDOF模式开始校准
    uint8_t ndof_mode = NDOF_MODE;
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, BNO055_REG_OPR_MODE,
                      I2C_MEMADD_SIZE_8BIT, &ndof_mode, 1, HAL_MAX_DELAY);
    HAL_Delay(30);
    // 监控校准进度
    uint8_t accel_cal = 0;

    do {
        BNO055_ReadCalibStatus(hi2c, &calib);
        accel_cal = calib.accel;
        debug_print("Accelerometer calibration progress: %d/3", accel_cal);

        if (accel_cal == 3) {
            debug_print(" V Done!\r\n");
        } else {
            debug_print(" O Continue...\r\n");
        }

        HAL_Delay(10000);  // 每10秒检查一次
    } while (accel_cal < 3);

    debug_print("Accelerometer calibration completed!\r\n");
    // 恢复原始模式
    HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, BNO055_REG_OPR_MODE,
                      I2C_MEMADD_SIZE_8BIT, &original_mode, 1, HAL_MAX_DELAY);
    HAL_Delay(30);
}

/**
 * @brief 验证FAST-LIO数据质量
 */
void VerifyFASTLIODataQuality(void)
{
    debug_print("\r\n=== FAST-LIO Data Quality Assessment ===\r\n");

    // 检查加速度数据
    if (fabs(BNO055_Data.AccZ / 100.0 - 9.8) < 0.5 && // Z轴接近9.8
            fabs(BNO055_Data.AccX / 100.0) < 0.5 &&        // X轴接近0
            fabs(BNO055_Data.AccY / 100.0) < 0.5) {        // Y轴接近0
        debug_print("V Acceleration data: EXCELLENT for FAST-LIO\r\n");
    }

    // 检查陀螺仪数据
    if (ABS(BNO055_Data.gyro1) < 0.01 &&
            ABS(BNO055_Data.gyro2) < 0.01 &&
            ABS(BNO055_Data.gyro3) < 0.01) {
        debug_print("V Gyroscope data: EXCELLENT for FAST-LIO\r\n");
    }

    debug_print("Overall assessment: V READY for FAST-LIO mapping!\r\n");
}

uint8_t BNO055_DebugReadIMUForFASTLIO(I2C_HandleTypeDef *hi2c, float gyro[3], float accel[3])
{
    uint8_t gyro_buffer[6], accel_buffer[6];

    // 读取角速度数据
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x14,
                         I2C_MEMADD_SIZE_8BIT, gyro_buffer, 6, HAL_MAX_DELAY) != HAL_OK) {
        debug_print("[BNO055] ERROR: Failed to read gyroscope data!\r\n");
        return 1;
    }

    // 读取加速度数据
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x08,
                         I2C_MEMADD_SIZE_8BIT, accel_buffer, 6, HAL_MAX_DELAY) != HAL_OK) {
        debug_print("[BNO055] ERROR: Failed to read accelerometer data!\r\n");
        return 1;
    }

    // 调试信息：原始字节数据
    debug_print("[BNO055] Gyro raw bytes: ");

    for (int i = 0; i < 6; i++) {
        debug_print("%02X ", gyro_buffer[i]);
    }

    debug_print("\r\n");
    debug_print("[BNO055] Accel raw bytes: ");

    for (int i = 0; i < 6; i++) {
        debug_print("%02X ", accel_buffer[i]);
    }

    debug_print("\r\n");
    // 转换前的原始整数值
    int16_t gyro_raw[3], accel_raw[3];
    gyro_raw[0] = (int16_t)((gyro_buffer[1] << 8) | gyro_buffer[0]);
    gyro_raw[1] = (int16_t)((gyro_buffer[3] << 8) | gyro_buffer[2]);
    gyro_raw[2] = (int16_t)((gyro_buffer[5] << 8) | gyro_buffer[4]);
    accel_raw[0] = (int16_t)((accel_buffer[1] << 8) | accel_buffer[0]);
    accel_raw[1] = (int16_t)((accel_buffer[3] << 8) | accel_buffer[2]);
    accel_raw[2] = (int16_t)((accel_buffer[5] << 8) | accel_buffer[4]);
    debug_print("[BNO055] Gyro raw values: X=%d, Y=%d, Z=%d\r\n",
                gyro_raw[0], gyro_raw[1], gyro_raw[2]);
    debug_print("[BNO055] Accel raw values: X=%d, Y=%d, Z=%d\r\n",
                accel_raw[0], accel_raw[1], accel_raw[2]);
    // 转换角速度数据 (rad/s)
    gyro[0] = gyro_raw[0] / 16.0 * (M_PI / 180.0);
    gyro[1] = gyro_raw[1] / 16.0 * (M_PI / 180.0);
    gyro[2] = gyro_raw[2] / 16.0 * (M_PI / 180.0);
    // 转换加速度数据 (m/s²)
    accel[0] = accel_raw[0] / 100.0;
    accel[1] = accel_raw[1] / 100.0;
    accel[2] = accel_raw[2] / 100.0;
    // 调试信息：转换后的值和中间计算
    debug_print("[BNO055] Gyro conversion: ");
    debug_print("Raw=%d -> DPS=%.3f -> Rad/s=%.6f\r\n",
                gyro_raw[0], gyro_raw[0] / 16.0, gyro[0]);
    debug_print("[BNO055] Accel conversion: ");
    debug_print("Raw=%d -> m/s2=%.3f\r\n", accel_raw[0], accel[0]);
    // 最终FAST-LIO数据输出
    debug_print("[BNO055] FAST-LIO Gyro (rad/s): X=%.6f, Y=%.6f, Z=%.6f\r\n",
                gyro[0], gyro[1], gyro[2]);
    debug_print("[BNO055] FAST-LIO Accel (m/s2): X=%.3f, Y=%.3f, Z=%.3f\r\n",
                accel[0], accel[1], accel[2]);

    // 数据质量检查
    if (fabs(accel[2] - 9.8) > 2.0) {
        debug_print("[BNO055] WARNING: Z acceleration (%.2f) deviates from gravity!\r\n", accel[2]);
    }

    if (fabs(gyro[0]) > 1.0 || fabs(gyro[1]) > 1.0 || fabs(gyro[2]) > 1.0) {
        debug_print("[BNO055] WARNING: High gyro values detected!\r\n");
    }

    // 更新全局数据结构
    BNO055_Data.gyro1 = gyro[0];
    BNO055_Data.gyro2 = gyro[1];
    BNO055_Data.gyro3 = gyro[2];
    BNO055_Data.AccX = accel[0] * 100.0;  // 转换为原始值存储
    BNO055_Data.AccY = accel[1] * 100.0;
    BNO055_Data.AccZ = accel[2] * 100.0;
    debug_print("[BNO055] Data read successful and global struct updated\r\n");
    debug_print("----------------------------------------\r\n");
    return 0;
}


uint8_t BNO055_ReadIMUForFASTLIO1(I2C_HandleTypeDef *hi2c, float gyro[3], float accel[3])
{
    uint8_t gyro_buffer[6], accel_buffer[6];

    // 读取角速度数据
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x14,
                         I2C_MEMADD_SIZE_8BIT, gyro_buffer, 6, HAL_MAX_DELAY) != HAL_OK) {
        return 1;
    }

    // 读取加速度数据
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x08,
                         I2C_MEMADD_SIZE_8BIT, accel_buffer, 6, HAL_MAX_DELAY) != HAL_OK) {
        return 1;
    }

    // 转换角速度数据 (rad/s)
    gyro[0] = ((int16_t)((gyro_buffer[1] << 8) | gyro_buffer[0])) / 16.0 * (M_PI / 180.0);
    gyro[1] = ((int16_t)((gyro_buffer[3] << 8) | gyro_buffer[2])) / 16.0 * (M_PI / 180.0);
    gyro[2] = ((int16_t)((gyro_buffer[5] << 8) | gyro_buffer[4])) / 16.0 * (M_PI / 180.0);
    // 转换加速度数据 (m/s²)
    accel[0] = ((int16_t)((accel_buffer[1] << 8) | accel_buffer[0])) / 100.0;
    accel[1] = ((int16_t)((accel_buffer[3] << 8) | accel_buffer[2])) / 100.0;
    accel[2] = ((int16_t)((accel_buffer[5] << 8) | accel_buffer[4])) / 100.0;
    // 更新全局数据结构（浮点数）
    BNO055_Data.gyro1 = gyro[0];
    BNO055_Data.gyro2 = gyro[1];
    BNO055_Data.gyro3 = gyro[2];
    BNO055_Data.AccX = ((int16_t)((accel_buffer[1] << 8) | accel_buffer[0]));  // 存储原始值
    BNO055_Data.AccY = ((int16_t)((accel_buffer[3] << 8) | accel_buffer[2]));
    BNO055_Data.AccZ = ((int16_t)((accel_buffer[5] << 8) | accel_buffer[4]));
    // 同时更新整型陀螺仪数据
    BNO055_Data.GyroX = ((int16_t)((gyro_buffer[1] << 8) | gyro_buffer[0]));
    BNO055_Data.GyroY = ((int16_t)((gyro_buffer[3] << 8) | gyro_buffer[2]));
    BNO055_Data.GyroZ = ((int16_t)((gyro_buffer[5] << 8) | gyro_buffer[4]));
    return 0;
}
